
#include "stanley.h"

#include <cmath>
#include <algorithm>

void StanleyController::FollowThread() {
    while (true) {

        follow_state_ = FollowState::kFollowing;

        // 计算线速度和转向角
        int   closest_idx = FindClosestPoint();
        float steer_angle = CalcSteeringAngle(closest_idx);
        float linear_vel  = CalcLinearVel(steer_angle);

        linear_vel_  = linear_vel;
        steer_angle_ = steer_angle;

        // 到达目标，停止跟随
        if (isReachGoal()) {
            follow_state_ = FollowState::kReachGoal;
            // 等待5个控制周期后，将控制器设置成空闲状态
            std::this_thread::sleep_for(std::chrono::milliseconds((int) (control_period_ * 5 * 1000)));
            follow_state_ = FollowState::kIdle;
            break;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(int(control_period_ * 1000)));
    }
}

// stanley核心函数，根据横向误差和航向误差计算转向角
float StanleyController::CalcSteeringAngle(int lookahead_idx) {
    auto target = path_[lookahead_idx];

    // 1. 计算横向误差（投影到路径法线方向）
    float dx                = target.x - pose_.x;
    float dy                = target.y - pose_.y;
    float cross_track_error = -dx * sin(target.yaw) + dy * cos(target.yaw);

    // 2. 计算航向误差
    float heading_error = normalizeAngle(target.yaw - pose_.yaw);

    // 3. 计算期望转向角（核心公式）
    float cross_error = atan(k_ * cross_track_error / (pose_.v + softener_));
    float steer_angle = 0.5 * heading_error + cross_error;

    // 4. 限制转向范围
    steer_angle = normalizeAngle(steer_angle);
    steer_angle = std::clamp(steer_angle, -max_steer_angle_, max_steer_angle_);

    return steer_angle;
}

// 根据当前转角、最大线速度和最大转角速度计算输出的线速度
float StanleyController::CalcLinearVel(float steer_angle) {
    float linear_vel = ((0.2 - max_linear_vel_) / max_steer_angle_) * std::fabs(steer_angle) + max_linear_vel_;

    // 距离终点比较近，进行减速
    float dx   = pose_.x - path_.back().x;
    float dy   = pose_.y - path_.back().y;
    float dist = std::sqrt(dx * dx + dy * dy);
    if (dist < 1.5) {
        linear_vel = 0.2;
    }

    // 曲率比较大的点也进行减速
    // int   closest_idx = FindClosestPoint();
    // float curvature   = path_[closest_idx].curvature;
    // if (path_[closest_idx].curvature > 0.1) {
    //     linear_vel = 0.2;
    // }

    return linear_vel;
}